/********************************************************************
Crytek Source File.
Copyright (C), Crytek Studios, 2001-2009.
-------------------------------------------------------------------------
File name:   WaypointHumanNavRegion.cpp
$Id$
Description: 

-------------------------------------------------------------------------
History:
- ?

*********************************************************************/

#include "StdAfx.h"
#include "WaypointHumanNavRegion.h"
#include "AICollision.h"
#include "CAISystem.h"
#include "ISerialize.h"
#include "WorldOctree.h"
#include "ISystem.h"
#include "IPhysics.h"
#include "I3DEngine.h"
#include "DebugDrawContext.h"	
#include "ITimer.h"
#include <limits>
#include "CalculationStopper.h"
#include "Walkability/WalkabilityCacheManager.h"

//#pragma optimize("", off)
//#pragma inline_depth(0)

static float zWaypointScale = 1.0f;

size_t CWaypointHumanNavRegion::s_instanceCount;
CWaypointHumanNavRegion::TNodes CWaypointHumanNavRegion::s_tmpNodes;

//====================================================================
// CWaypointHumanNavRegion
//====================================================================
CWaypointHumanNavRegion::CWaypointHumanNavRegion(CGraph* pGraph)
{
	++ s_instanceCount;

  AIAssert(pGraph);
  m_pGraph = pGraph;
  m_currentNodeIt = 0;
}

//====================================================================
// ~CWaypointHumanNavRegion
//====================================================================
CWaypointHumanNavRegion::~CWaypointHumanNavRegion()
{
  delete m_currentNodeIt;
  m_currentNodeIt = 0;

	-- s_instanceCount;
	if (!s_instanceCount)
		stl::free_container(s_tmpNodes);
}

//===================================================================
// ResetUpdateStatus
//===================================================================
void CWaypointHumanNavRegion::ResetUpdateStatus()
{
  delete m_currentNodeIt;
  m_currentNodeIt = 0;
}

//====================================================================
// UglifyPath
//====================================================================
void CWaypointHumanNavRegion::UglifyPath(const VectorConstNodeIndices& inPath, TPathPoints& outPath, 
                                         const Vec3& startPos, const Vec3& startDir, 
                                         const Vec3& endPos, const Vec3 & endDir)
{
  outPath.push_back(PathPointDescriptor(IAISystem::NAV_WAYPOINT_HUMAN, startPos));
  for(VectorConstNodeIndices::const_iterator itrCurNode=inPath.begin() ; itrCurNode != inPath.end() ; ++itrCurNode)
  {
		const GraphNode& curNode=*m_pGraph->GetNodeManager().GetNode(*itrCurNode);
    outPath.push_back(PathPointDescriptor(IAISystem::NAV_WAYPOINT_HUMAN, curNode.GetPos()));
  }
  outPath.push_back(PathPointDescriptor(IAISystem::NAV_WAYPOINT_HUMAN, endPos));
}

//====================================================================
// BeautifyPath
//====================================================================
void CWaypointHumanNavRegion::BeautifyPath(const VectorConstNodeIndices& inPath, TPathPoints& outPath, 
                                           const Vec3& startPos, const Vec3& startDir, 
                                           const Vec3& endPos, const Vec3 & endDir,
                                           float radius,
                                           const AgentMovementAbility & movementAbility,
                                           const TNavigationBlockers& navigationBlockers)
{
  FUNCTION_PROFILER( GetISystem(), PROFILE_AI );
	if (inPath.empty())
    return;

  AIAssert(m_pGraph->GetNodeManager().GetNode(inPath.front())->navType == IAISystem::NAV_WAYPOINT_HUMAN);
  int nBuildingID = m_pGraph->GetNodeManager().GetNode(inPath.front())->GetWaypointNavData()->nBuildingID;
  if (nBuildingID < 0)
  {
    AIWarning("CWaypointHumanNavRegion::BeautifyPath Unable to get building ID from node %d", inPath.front());
    return;
  }

  const SpecialArea* sa = gAIEnv.pNavigation->GetSpecialArea(nBuildingID);
  if (!sa)
  {
    AIWarning("CWaypointHumanNavRegion::BeautifyPath Unable to get special area for building ID %d", nBuildingID);
    return;
  }
  const ListPositions& buildingPolygon = sa->GetPolygon();

  bool allowStartSkip = !startPos.IsEquivalent(m_pGraph->GetNodeManager().GetNode(inPath.front())->GetPos(), 0.01f);
  bool allowEndSkip = !endPos.IsEquivalent(m_pGraph->GetNodeManager().GetNode(inPath.back())->GetPos(), 0.01f);

  if (inPath.size() == 1)
  {
    if (allowStartSkip && allowEndSkip)
    {
			// we can walk directly - don't need to go via the node.
			if (gAIEnv.pWalkabilityCacheManager->CheckWalkability(0, startPos, endPos, movementAbility.pathRadius,
				buildingPolygon, 0, 0, &sa->GetAABB()))
				return;
    }
  }

  VectorConstNodeIndices inPathCopy(inPath);
  // see if we can skip the last one
  unsigned inPathSize = inPathCopy.size();

  if (inPathSize > 1 && allowEndSkip)
  {
    if (gAIEnv.pWalkabilityCacheManager->CheckWalkability(0, endPos, 
			m_pGraph->GetNodeManager().GetNode(inPath[inPathSize-2])->GetPos(), 
			movementAbility.pathRadius))
    {
      inPathCopy.pop_back();
      --inPathSize;
    }
  }
  // see if we can skip the first one
  if (inPathSize > 1 && allowStartSkip)
  {
    if (gAIEnv.pWalkabilityCacheManager->CheckWalkability(0, startPos, m_pGraph->GetNodeManager().GetNode(inPath[1])->GetPos(), 
			movementAbility.pathRadius))
    {
      inPathCopy.erase(inPathCopy.begin());
      --inPathSize;
    }
  }
  else if (inPathSize == 1 && allowStartSkip)
  {
    if (gAIEnv.pWalkabilityCacheManager->CheckWalkability(0, startPos, endPos, movementAbility.pathRadius))
    {
      // we can walk directly - don't need to go via the node.
      return;
    }
  }

  // Just go through all the nodes
  for(VectorConstNodeIndices::const_iterator itrCurNode=inPathCopy.begin() ; itrCurNode != inPathCopy.end() ; ++itrCurNode)
  {
    const GraphNode& curNode=*m_pGraph->GetNodeManager().GetNode(*itrCurNode);
    AIAssert(curNode.navType == IAISystem::NAV_WAYPOINT_HUMAN);
    outPath.push_back(PathPointDescriptor(IAISystem::NAV_WAYPOINT_HUMAN, curNode.GetPos()));
  }
}

//====================================================================
// FillGreedyMap
//====================================================================
void CWaypointHumanNavRegion::FillGreedyMap(unsigned nodeIndex, const Vec3 &pos, 
                                            IVisArea *pTargetArea, bool bStayInArea, CandidateIdMap& candidates)
{
	GraphNode* pNode = m_pGraph->GetNodeManager().GetNode(nodeIndex);
  m_pGraph->MarkNode(nodeIndex);

  if (!pNode->firstLinkIndex)
    return;
  for (unsigned link = pNode->firstLinkIndex; link; link = m_pGraph->GetLinkManager().GetNextLink(link))
  { 
		unsigned nowIndex = m_pGraph->GetLinkManager().GetNextNode(link);
    GraphNode *pNow = m_pGraph->GetNodeManager().GetNode(nowIndex);
    if ( pNow->mark || pNow->navType != IAISystem::NAV_WAYPOINT_HUMAN || pNow->GetWaypointNavData()->nBuildingID != pNode->GetWaypointNavData()->nBuildingID) 
      continue;
    if (bStayInArea && (pNow->GetWaypointNavData()->pArea != pTargetArea))
      continue;

    Vec3 delta = (pos - pNow->GetPos());
    delta.z *= zWaypointScale; // scale vertical to prefer waypoints on the same level
    float thisdist = delta.GetLengthSquared();
    candidates.insert(CandidateIdMap::iterator::value_type(thisdist,nowIndex));

    // this snippet will make sure we only check all points inside the target area - not the whole building
    if (pTargetArea && pNow->GetWaypointNavData()->pArea == pTargetArea)
      FillGreedyMap(nowIndex,pos,pTargetArea,true, candidates);
    else
      FillGreedyMap(nowIndex,pos,pTargetArea,false, candidates);
  }

}

struct SWrongBuildingRadius
{
  SWrongBuildingRadius(CGraphLinkManager& linkManager, CGraphNodeManager& nodeManager, int ID, float radius)
		: linkManager(linkManager), nodeManager(nodeManager), ID(ID), radius(radius) {}
  bool operator()(const std::pair<float, unsigned> &node) 
  {
    if (nodeManager.GetNode(node.second)->GetWaypointNavData()->nBuildingID != ID)
      return true;
    if (GetAISystem()->ExitNodeImpossible(linkManager, nodeManager.GetNode(node.second), radius))
      return true;
    return false;
  }
	CGraphLinkManager& linkManager;
	CGraphNodeManager& nodeManager;
  int ID;
  float radius;
};

//====================================================================
// GetEnclosing
//====================================================================
unsigned CWaypointHumanNavRegion::GetEnclosing(const Vec3 &pos, float passRadius, unsigned startIndex, 
                                                 float range, Vec3 * closestValid, bool returnSuspect, const char *requesterName, bool omitWalkabilityTest)
{
  FUNCTION_PROFILER( GetISystem(), PROFILE_AI );

	// A range value less than zero results in get search based on fNodeAutoConnectDistance.
	if (range < 0.0f)
		range = 0.0f;

  int	nBuildingID;
  gAIEnv.pNavigation->CheckNavigationType(pos, nBuildingID, IAISystem::NAV_WAYPOINT_HUMAN);

  if (nBuildingID < 0)
  {
    AIWarning("CWaypointHumanNavRegion::GetEnclosing found bad building ID = %d (%s) for %s (%5.2f, %5.2f, %5.2f)", 
      nBuildingID, gAIEnv.pNavigation->GetSpecialAreaName(nBuildingID), requesterName, pos.x, pos.y, pos.z);
    return 0;
  }

  const SpecialArea *sa = gAIEnv.pNavigation->GetSpecialArea(nBuildingID);
  if (!sa)
  {
    AIWarning("CWaypointHumanNavRegion::GetEnclosing found no area for %s (%5.2f, %5.2f, %5.2f)", 
      requesterName, pos.x, pos.y, pos.z);
    return 0;
  }

  // get a floor position
  Vec3 floorPos;
  static float maxFloorDist = 15.0f; // big value because the target might be huge - e.g. hunter
  if (!omitWalkabilityTest && false == gAIEnv.pWalkabilityCacheManager->FindFloor(0, pos, floorPos))
  {
    AIWarning("CWaypointHumanNavRegion::GetEnclosing Could not find floor position from (%5.2f, %5.2f, %5.2f) %s",
      pos.x, pos.y, pos.z, requesterName);
    return 0;
  }
	else if (omitWalkabilityTest)
	{
		floorPos = pos;
	}

  if (sa->fNodeAutoConnectDistance < 0.0001f)
    AIWarning("CWaypointHumanNavRegion::GetEnclosing AI nav modifier %s has NodeAutoConnectDistance=0 - increase to a sensible value [design bug]", gAIEnv.pNavigation->GetSpecialAreaName(nBuildingID));

  float totalDist = 1.5f * sa->fNodeAutoConnectDistance + range;

	TNodes& nodes = s_tmpNodes;
  nodes.resize(0);
	
  // Most times we'll find the desired node close by
  CAllNodesContainer &allNodes = m_pGraph->GetAllNodes();
  allNodes.GetAllNodesWithinRange(nodes, floorPos, totalDist, IAISystem::NAV_WAYPOINT_HUMAN);
  std::vector< std::pair<float, unsigned> >::iterator it = std::remove_if(nodes.begin(), nodes.end(), SWrongBuildingRadius(m_pGraph->GetLinkManager(), m_pGraph->GetNodeManager(), nBuildingID, passRadius));
  nodes.erase(it, nodes.end());

  if (nodes.empty())
  {
    AILogComment("Unable to find node close to %s (%5.2f, %5.2f, %5.2f) - checking all nodes from building id %d (%s)",
      requesterName, pos.x, pos.y, pos.z, nBuildingID, gAIEnv.pNavigation->GetSpecialAreaName(nBuildingID));

    CAllNodesContainer::Iterator nit(allNodes, IAISystem::NAV_WAYPOINT_HUMAN);
    while (unsigned currentIndex = nit.Increment())
    {
			GraphNode* pCurrent = m_pGraph->GetNodeManager().GetNode(currentIndex);
      if (pCurrent->GetWaypointNavData()->nBuildingID == nBuildingID && !GetAISystem()->ExitNodeImpossible(m_pGraph->GetLinkManager(), pCurrent, passRadius))
      {
        nodes.push_back(std::make_pair(0.0f, currentIndex)); // dist calculated later
      }
    }
  }

  if (nodes.empty())
  {
    AIWarning("CWaypointHumanNavRegion::GetEnclosing No nodes found for area containing %s (%5.2f, %5.2f, %5.2f) building id %d (%s) [design bug]", 
      requesterName, pos.x, pos.y, pos.z, nBuildingID, gAIEnv.pNavigation->GetSpecialAreaName(nBuildingID));
    return 0;
  }

  unsigned closestIndex = 0;
	float closestDist = std::numeric_limits<float>::max();

	for (unsigned i = 0 ; i < nodes.size() ; ++i)
	{
		Vec3 delta = m_pGraph->GetNodeManager().GetNode(nodes[i].second)->GetPos() - floorPos;
		delta.z *= 2.5f;
		float distSq = delta.GetLengthSquared();
		nodes[i].first = distSq;
		if (distSq < closestDist)
		{
			closestDist = distSq;
			closestIndex = nodes[i].second;
		}
	}

  // Assume that if the requester wants a full check then they've passed in a closest position
  // pointer - and if they haven't they're doing a quick test. The code gets called from 
  // COPTrace::Execute2D - Danny todo see if we can avoid calling this code from there, or call
  // a more specific function
//  if (!closestValid)
//    return pClosest;

	if (omitWalkabilityTest)
		return closestIndex;

	std::sort(nodes.begin(), nodes.end());

  const ListPositions& buildingPolygon = sa->GetPolygon();

  const uint32 MaxRejected = 10;
	uint32 rejected = 0;
	const float radiusTest = (passRadius >= 0.0001f) ? passRadius : WalkabilityRadius;

	for (TNodes::iterator nit = nodes.begin() ; nit != nodes.end() && rejected < MaxRejected; ++nit )
	{
		uint32 nodeIndex = nit->second;

		if (CheckWalkabilitySimple(SWalkPosition(floorPos, true), m_pGraph->GetNodeManager().GetNode(nodeIndex)->GetPos(), 
			radiusTest, AICE_ALL))
			return nodeIndex;

		++rejected;
	}

	// NOTE Jun 16, 2007: <pvl> draw the requester pos and pos of nodes that had
	// their walkability from 'pos' checked and failed
	float timeVisible = gAIEnv.CVars.DrawGetEnclosingFailures;
	if (timeVisible > 0.0f)
	{
		TNodes::iterator nit = nodes.begin();
		TNodes::iterator nend = nodes.end();
		for (int i=0; nit != nend && i < MaxRejected; ++nit, ++i )
		{
			uint32 nodeIndex = nit->second;
			Vec3 nodePos = m_pGraph->GetNodeManager().GetNode(nodeIndex)->GetPos();
			GetAISystem()->AddDebugLine (pos, nodePos, 255, 255, 0, timeVisible);
			GetAISystem()->AddDebugSphere (nodePos, 0.15f, 255, 255, 0, timeVisible);
		}
	}

  uint32 nodeIndex = closestIndex;
  if (returnSuspect)
	{
		AIWarning("CWaypointHumanNavRegion::GetEnclosing %d nodes rejected for %s reaching (pos: %5.2f, %5.2f, %5.2f) (floor: %5.2f, %5.2f, %5.2f): returning %d (in %d (%s))", 		 rejected, requesterName, pos.x, pos.y, pos.z, floorPos.x, floorPos.y, floorPos.z, nodeIndex, nBuildingID, gAIEnv.pNavigation->GetSpecialAreaName(nBuildingID));
	}

  return nodeIndex;
}

//====================================================================
// CheckPassability
//====================================================================
bool CWaypointHumanNavRegion::CheckPassability(const Vec3& from, const Vec3& to, float radius, const TNavigationBlockers& navigationBlockers, IAISystem::tNavCapMask ) const
{
	FUNCTION_PROFILER( GetISystem(), PROFILE_AI );

	return gAIEnv.pWalkabilityCacheManager->CheckWalkability(0, from, to, radius);
}

//====================================================================
// GetClosestNode
//====================================================================
unsigned CWaypointHumanNavRegion::GetClosestNode(const Vec3 &pos, int nBuildingID)
{
  FUNCTION_PROFILER( GetISystem(), PROFILE_AI );
  std::vector<unsigned> entrances;
  if (!m_pGraph->GetEntrances(nBuildingID, pos, entrances))
  {
    AIWarning("CWaypointHumanNavRegion::GetClosestNode No entrance for navigation area nr %d (%s). The position is (%.3f,%.3f,%.3f)",
      nBuildingID, gAIEnv.pNavigation->GetSpecialAreaName(nBuildingID), pos.x, pos.y, pos.z);
    return 0;
  }

  IVisArea *pGoalArea = 0; 
  m_pGraph->ClearMarks();
  CandidateIdMap candidates;
  for (unsigned i = 0 ; i < entrances.size() ; ++i)
    FillGreedyMap(entrances[i], pos, pGoalArea, false, candidates);
  m_pGraph->ClearMarks();

  if (candidates.empty())
  {
    AIWarning("CWaypointHumanNavRegion::GetClosestNode No nodes found for this navigation modifier %d (%s) apart from entrance",
      nBuildingID, gAIEnv.pNavigation->GetSpecialAreaName(nBuildingID));
    return 0;
  }
  else
  {
    return candidates.begin()->second;
  }
}

//====================================================================
// Serialize
//====================================================================
void CWaypointHumanNavRegion::Serialize(TSerialize ser)
{
  ser.BeginGroup("WaypointNavRegion");

  ser.EndGroup();
}

//====================================================================
// Reset
//====================================================================
void CWaypointHumanNavRegion::Clear()
{
  delete m_currentNodeIt;
  m_currentNodeIt = 0;
}

#ifdef CRYAISYSTEM_DEBUG
//====================================================================
// CheckAndDrawBody
//====================================================================
bool CWaypointHumanNavRegion::CheckAndDrawBody(const Vec3& pos)
{
  bool result = false;
  bool gotFloor = false;
  Vec3 floorPos = pos;
  if (GetFloorPos(floorPos, pos, WalkabilityFloorUpDist, WalkabilityFloorDownDist, WalkabilityDownRadius, AICE_ALL))
    gotFloor = true;

  Vec3 segStart = floorPos + WalkabilityTorsoBaseOffset + Vec3(0, 0, WalkabilityRadius);
  Vec3 segEnd = floorPos + Vec3(0, 0, WalkabilityTotalHeight - WalkabilityRadius); 
  Lineseg torsoSeg(segStart, segEnd);

  if (gotFloor && !OverlapCapsule(torsoSeg, WalkabilityRadius, AICE_ALL))
    result = true;

  CDebugDrawContext dc;
  {
    ColorF col;
    if (result)
      col.Set(0, 1, 0);
    else
      col.Set(1, 0, 0);
    dc->DrawSphere(segStart, WalkabilityRadius, col);
    dc->DrawSphere(segEnd, WalkabilityRadius, col);
    dc->DrawSphere(segStart, WalkabilityDownRadius, col);
    dc->DrawSphere(floorPos, WalkabilityDownRadius, col);
    dc->DrawLine(floorPos, col, segStart, col);
  }

  return result;
}
#endif //CRYAISYSTEM_DEBUG


/**
 * If simplified walkability computation isn't requested by a console variable,
 * this function just falls back to generic CheckWalkability() test.  If a link
 * is classified as "simple", its walkability is tested using
 * CheckWalkabilitySimple() which should be faster.  Conversely, if the link
 * fails the "simplicity" test, generic CheckWalkability() is invoked.
 *
 * If "simplicity" of the link hasn't been established yet, it is computed now.
 * The test involves gathering floor heights along the link and seeing if the
 * sequence is straight or "linear" enough.
 */
#pragma warning (push)
#pragma warning (disable: 6262)
bool CWaypointHumanNavRegion::CheckPassability(size_t linkIndex, Vec3 from, Vec3 to, float radius,
																							 const ListPositions& boundary, EAICollisionEntities aiCollisionEntities)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	if (Overlap::Lineseg_Polygon2D(Lineseg(from, to), boundary))
		return false;

	if (!gAIEnv.CVars.SimpleWayptPassability)
		return CheckWalkability(from, to, radius);

	bool simplicityComputed = m_pGraph->GetLinkManager().SimplicityComputed(linkIndex);

	if (simplicityComputed)
	{
		if (m_pGraph->GetLinkManager().IsSimple(linkIndex))
			return CheckWalkabilitySimple(from, to, radius, aiCollisionEntities);
	}

	float minZ = min(from.z - 0.75f, to.z - 0.75f);
	float maxZ = min(from.z + 0.75f, to.z + 0.75f) + WalkabilityTotalHeight;

	AABB enclosingAABB(AABB::RESET);
	enclosingAABB.Add(Vec3(from.x, from.y, minZ), radius + 0.001f);
	enclosingAABB.Add(Vec3(to.x, to.y, maxZ), radius + 0.001f);

	StaticPhysEntityArray entities;
	size_t entityCount = GetPhysicalEntitiesInBox(enclosingAABB.min, enclosingAABB.max, entities, (int)aiCollisionEntities);
	
	// no entities means also no floor, so impassable, and don't touch the cache
	if (!entityCount)
		return false;

	SCachedPassabilityResult* cached = GetCachedPassabilityResult(linkIndex);
	size_t hashValue = GetHashFromEntities(&entities.front(), entityCount);
	
	if (cached && (cached->spatialHash == hashValue))
		return cached->walkableResult;

	StaticAABBArray aabbs;
	aabbs.resize(entityCount);

	// get all aabbs
	{
		pe_status_pos status;

		for(size_t i = 0; i < entityCount; ++i)
		{			
			if (entities[i]->GetStatus(&status))
			{
				const Vec3 aabbMin = status.BBox[0];
				const Vec3 aabbMax = status.BBox[1];

				// some aabbs strangely return all zero from physics, thus use the enclosingAABB for the whole path
				if (!aabbMin.IsZero() && !aabbMax.IsZero())
					aabbs[i] = AABB(aabbMin + status.pos, aabbMax + status.pos);
				else
					aabbs[i] = AABB(enclosingAABB.min, enclosingAABB.max);				
			}
		}
	}

	bool simple = false;
	bool walkable = CheckWalkability(from, to, radius, entities, aabbs, 0, &simple);

	if (cached)
	{
		cached->spatialHash = hashValue;
		cached->walkableResult = walkable;
	}
	else
		InsertCachedPassabilityResult(linkIndex, SCachedPassabilityResult(hashValue, walkable));

	if (walkable && simple)
	{
		CGraphLinkManager& linkManager = ((CGraph*)gAIEnv.pGraph)->GetLinkManager();
		linkManager.SetSimple(linkIndex, true);
	}

	return walkable;
}
#pragma warning (pop)

//===================================================================
// ModifyNodeConnections
//===================================================================
void CWaypointHumanNavRegion::ModifyNodeConnections(unsigned nodeIndex1, EAICollisionEntities collisionEntities, unsigned link, bool adjustOriginalEditorLinks)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	GraphNode* pNode1 = m_pGraph->GetNodeManager().GetNode(nodeIndex1);
	unsigned nodeIndex2 = m_pGraph->GetLinkManager().GetNextNode(link);
  GraphNode *pNode2 = m_pGraph->GetNodeManager().GetNode(nodeIndex2);
  if (pNode2 < pNode1)
  {
    return;
  }
  else
  {
    int16 origRadiusCm = m_pGraph->GetLinkManager().GetOrigRadiusInCm(link);
    EWaypointLinkType origType = SWaypointNavData::GetLinkTypeFromRadiusInCm(origRadiusCm);

    int16 curRadiusCm = m_pGraph->GetLinkManager().GetRadiusInCm(link);
    EWaypointLinkType curType = SWaypointNavData::GetLinkTypeFromRadiusInCm(curRadiusCm);

    bool walkabilityFromTo = false;

    if (origType == WLT_EDITOR_IMPASS)
    {
      // don't over-ride editor impassability
      walkabilityFromTo = false;
    }
    else
    {
      const SpecialArea *sa = gAIEnv.pNavigation->GetSpecialArea(pNode1->GetWaypointNavData()->nBuildingID);
      if (!sa)
        return;

      const ListPositions	& buildingPolygon = sa->GetPolygon();
/*
      Vec3 nodeOffset(0, 0, 0.5f);
      walkabilityFromTo = CheckWalkability(
        pNode1->GetPos() + nodeOffset, 
        pNode2->GetPos() + nodeOffset, 
        0.0f, true, buildingPolygon, collisionEntities,
				GetCachedPassabilityResult(link, true));
				*/
    }

    if ( (walkabilityFromTo && curRadiusCm < 0) || (!walkabilityFromTo && curRadiusCm > 0) )
    {
      int16 newRadius = 0;

      if (curType == WLT_AUTO_IMPASS || curType == WLT_AUTO_PASS)
      {
        unsigned linkOneTwo = 0;
        unsigned linkTwoOne = 0;

				newRadius = walkabilityFromTo ? WLT_AUTO_PASS * 100 : WLT_AUTO_IMPASS * 100;

        m_pGraph->ConnectInCm(nodeIndex1, nodeIndex2, newRadius, newRadius, &linkOneTwo, &linkTwoOne);
        if (origType == WLT_AUTO_PASS && adjustOriginalEditorLinks)
        {
          if (linkOneTwo)
            m_pGraph->GetLinkManager().SetRadiusInCm(linkOneTwo, newRadius);
          if (linkTwoOne)
            m_pGraph->GetLinkManager().SetRadiusInCm(linkTwoOne, newRadius);
        }
      }
      else if (curType == WLT_EDITOR_IMPASS || curType == WLT_EDITOR_PASS)
      {
        newRadius = walkabilityFromTo ? WLT_EDITOR_PASS * 100 : WLT_EDITOR_IMPASS * 100;
        m_pGraph->ConnectInCm(nodeIndex1, nodeIndex2, newRadius, newRadius);
      }
    } // walkability has changed 
  } // link pointer
}

// NOTE Mai 6, 2007: <pvl> my dbg stuff - keeping it in for now, might be
// useful in future
/*
#include <cstdio>

class ModConnTimer {
		CTimeValue mStart;
		static float sAccumTime;
		static int sInvocations;
		static FILE * sFile;
public:
		ModConnTimer()
		{
			mStart = gEnv->pTimer->GetAsyncTime();
			++sInvocations;
			if (sFile == 0) sFile = fopen ("mrd", "w");
		}
		~ModConnTimer()
		{
			CTimeValue end = gEnv->pTimer->GetAsyncTime();
			sAccumTime += (end - mStart).GetMilliSeconds();

			fprintf (sFile, "modconn ran for %f ms\n", (end-mStart).GetMilliSeconds());
			fflush (sFile);
		}
		static void Reset ()
		{
			fprintf (sFile, "%f - %d\n", sAccumTime, sInvocations);
			fflush (sFile);
			sAccumTime = 0.0f;
			sInvocations = 0;
		}
};

float ModConnTimer::sAccumTime = 0.0f;
int ModConnTimer::sInvocations = 0;
FILE * ModConnTimer::sFile = 0;
*/

//====================================================================
// ModifyConnections
//====================================================================
void CWaypointHumanNavRegion::ModifyConnections(const CCalculationStopper& stopper, EAICollisionEntities collisionEntities)
{
  FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

  CAllNodesContainer& allNodes = m_pGraph->GetAllNodes();
	const CGraphLinkManager& linkManager = m_pGraph->GetLinkManager();
	const CGraphNodeManager& nodeManager = m_pGraph->GetNodeManager();
  
	if (!m_currentNodeIt)
  {
    m_currentNodeIt = new CAllNodesContainer::Iterator(allNodes, IAISystem::NAV_WAYPOINT_HUMAN);
		m_currentLink = (m_currentNodeIt->GetNodeIndex() ? nodeManager.GetNode(m_currentNodeIt->GetNodeIndex())->firstLinkIndex : 0);
	}

  size_t origNodeIndex = 0;
  bool origNodeLeft = false;

	CAIObject* player = GetAISystem()->GetPlayer();
	const Vec3 playerLocation = player ? player->GetPos() : gEnv->pSystem->GetViewCamera().GetPosition();
	
  do
  {
    size_t nodeIndex1 = m_currentNodeIt->GetNodeIndex();

    if (!nodeIndex1)
    {
      m_currentNodeIt->Reset();
      nodeIndex1 = m_currentNodeIt->GetNodeIndex();

			m_currentLink = (nodeIndex1 ? nodeManager.GetNode(nodeIndex1)->firstLinkIndex : 0);

      if (!nodeIndex1)
        return;
    }

    // prevent sitting in a wasteful loop if there's no more to do
    if (!origNodeIndex)
      origNodeIndex = nodeIndex1;
    else if (origNodeIndex == nodeIndex1 && origNodeLeft)
      return;

		const GraphNode* pNode1 = nodeManager.GetNode(nodeIndex1);

		int buildingID1 = pNode1->GetWaypointNavData()->nBuildingID;
    const SpecialArea* specialArea1 = gAIEnv.pNavigation->GetSpecialArea(buildingID1);

    if (!m_currentLink || !specialArea1)
    {
      m_currentNodeIt->Increment();
			if (m_currentNodeIt->GetNodeIndex())
	      m_currentLink = nodeManager.GetNode(m_currentNodeIt->GetNodeIndex())->firstLinkIndex;;

      origNodeLeft = true;
    }
    else
    {
      size_t link = m_currentLink;
			size_t nodeIndex2 = linkManager.GetNextNode(link);
      const GraphNode *pNode2 = nodeManager.GetNode(nodeIndex2);

      if (pNode2->navType != IAISystem::NAV_WAYPOINT_HUMAN)
      {
        m_currentLink = linkManager.GetNextLink(m_currentLink);
        continue;
      }

      int buildingID2 = pNode2->GetWaypointNavData()->nBuildingID;
      const SpecialArea* specialArea2 = gAIEnv.pNavigation->GetSpecialArea(buildingID2);
      
			if (!specialArea2)
      {
        m_currentLink = linkManager.GetNextLink(m_currentLink);
        continue;
      }

      bool auto1 = specialArea1->waypointConnections == WPCON_DESIGNER_PARTIAL || 
				specialArea1->waypointConnections == WPCON_AUTO_PARTIAL;
      bool auto2 = specialArea2->waypointConnections == WPCON_DESIGNER_PARTIAL || 
				specialArea2->waypointConnections == WPCON_AUTO_PARTIAL;

      // only check one way for each pair
      // also only check if at least one is auto
      if (pNode2 < pNode1 || (!auto1 && !auto2))
      {
        m_currentLink = linkManager.GetNextLink(m_currentLink); // only check one way for each pair
        continue;
      }
      else
      {
				int16 origRadiusInCm = linkManager.GetOrigRadiusInCm(link);
				int16 curRadiusInCm = linkManager.GetRadiusInCm(link);

				bool critterOnly = (specialArea1->bCritterOnly || specialArea2->bCritterOnly);

				EWaypointLinkType origType, curType;
				if (critterOnly)
				{
					origType = origRadiusInCm > 0 ? WLT_AUTO_PASS : WLT_AUTO_IMPASS;
					curType = curRadiusInCm > 0 ? WLT_AUTO_PASS : WLT_AUTO_IMPASS;
				}
				else
				{
					origType = SWaypointNavData::GetLinkTypeFromRadiusInCm(origRadiusInCm);
					curType = SWaypointNavData::GetLinkTypeFromRadiusInCm(curRadiusInCm);
				}

				bool walkabilityFromTo = false;
					
        if (origType != WLT_EDITOR_IMPASS)
        {
					const Vec3 node1Location = pNode1->GetPos();
					const Vec3 node2Location = pNode2->GetPos();

					float t;
					const float distanceToPlayerSq = Distance::Point_LinesegSq(playerLocation, Lineseg(node1Location, node2Location), t);

          if (distanceToPlayerSq > sqr(65.0f))
						walkabilityFromTo = linkManager.GetRadiusInCm(link) > 0;
					else if ((node1Location - node2Location).len2() <= sqr(25.0f))
					{
						const ListPositions& buildingPolygon = specialArea1->GetPolygon();
						const Vec3 nodeOffset(0.0f, 0.0f, 0.5f);

						float testRadius = critterOnly ? 0.15f : 0.35f;

						walkabilityFromTo = CheckPassability(link, node1Location + nodeOffset, node2Location + nodeOffset, 
							testRadius, buildingPolygon, collisionEntities);
					}
        }



        if ((walkabilityFromTo && curRadiusInCm <= 0) || (!walkabilityFromTo && curRadiusInCm > 0) ||
					(critterOnly && walkabilityFromTo && (curRadiusInCm > 15)))
				{
          int16 newRadius = 0;

          if (curType == WLT_AUTO_IMPASS || curType == WLT_AUTO_PASS)
          {
						int16 passableRadiusCm = critterOnly ? 15 : (WLT_AUTO_PASS * 100);

            newRadius = walkabilityFromTo ? passableRadiusCm : WLT_AUTO_IMPASS * 100;
            m_pGraph->ConnectInCm(nodeIndex1, nodeIndex2, newRadius, newRadius);
          }
          else if (curType == WLT_EDITOR_IMPASS || curType == WLT_EDITOR_PASS)
          {
						int16 passableRadiusCm = critterOnly ? 15 : (WLT_EDITOR_PASS * 100);

            newRadius = walkabilityFromTo ? passableRadiusCm : WLT_EDITOR_IMPASS * 100;
            m_pGraph->ConnectInCm(nodeIndex1, nodeIndex2, newRadius, newRadius);
          }
        }
      }

      m_currentLink = linkManager.GetNextLink(m_currentLink);
    }
  } while (!stopper.ShouldCalculationStop());
}


//====================================================================
// Update
//====================================================================
void CWaypointHumanNavRegion::Update(CCalculationStopper& stopper)
{
  FUNCTION_PROFILER( GetISystem(), PROFILE_AI );

  ModifyConnections(stopper, /*AICE_ALL_EXCEPT_TERRAIN_AND_STATIC*/AICE_ALL);
}

//====================================================================
// Reset
//====================================================================
void CWaypointHumanNavRegion::Reset(IAISystem::EResetReason reason)
{
	PassabilityCache().swap(m_passabilityCache);
}

//===================================================================
// GetTeleportPosition
//===================================================================
bool CWaypointHumanNavRegion::GetTeleportPosition(const Vec3 &pos, Vec3 &teleportPos, const char *requesterName)
{
  FUNCTION_PROFILER( GetISystem(), PROFILE_AI );

  teleportPos.zero();

  int	nBuildingID;
  gAIEnv.pNavigation->CheckNavigationType(pos, nBuildingID, IAISystem::NAV_WAYPOINT_HUMAN);

  if (nBuildingID < 0)
  {
    AIWarning("CWaypointHumanNavRegion::GetTeleportPosition found bad building ID = %d (%s) for %s (%5.2f, %5.2f, %5.2f)", 
      nBuildingID, gAIEnv.pNavigation->GetSpecialAreaName(nBuildingID), requesterName, pos.x, pos.y, pos.z);
    return false;
  }

  const SpecialArea *sa = gAIEnv.pNavigation->GetSpecialArea(nBuildingID);
  if (!sa)
  {
    AIWarning("CWaypointHumanNavRegion::GetTeleportPosition found no area for %s (%5.2f, %5.2f, %5.2f)", 
      requesterName, pos.x, pos.y, pos.z);
    return false;
  }

  if (sa->fNodeAutoConnectDistance < 0.0001f)
    AIWarning("CWaypointHumanNavRegion::GetTeleportPosition AI nav modifier %s has NodeAutoConnectDistance=0 - increase to a sensible value [design bug]", gAIEnv.pNavigation->GetSpecialAreaName(nBuildingID));

  float totalDist = 5 * sa->fNodeAutoConnectDistance + 5.0f;

  typedef std::vector< std::pair<float, unsigned> > TNodes;
  static TNodes nodes;
  nodes.resize(0);

  // Most times we'll find the desired node close by
  CAllNodesContainer &allNodes = m_pGraph->GetAllNodes();
  allNodes.GetAllNodesWithinRange(nodes, pos, totalDist, IAISystem::NAV_WAYPOINT_HUMAN);
  std::vector< std::pair<float, unsigned> >::iterator it = std::remove_if(nodes.begin(), nodes.end(), SWrongBuildingRadius(m_pGraph->GetLinkManager(), m_pGraph->GetNodeManager(), nBuildingID, 0.4f));
  nodes.erase(it, nodes.end());

  if (nodes.empty())
  {
    AILogComment("CWaypointHumanNavRegion::GetTeleportPosition Unable to find node close to %s (%5.2f, %5.2f, %5.2f) - checking all nodes from building id %d (%s)",
      requesterName, pos.x, pos.y, pos.z, nBuildingID, gAIEnv.pNavigation->GetSpecialAreaName(nBuildingID));

    CAllNodesContainer::Iterator nit(allNodes, IAISystem::NAV_WAYPOINT_HUMAN);
    while (unsigned currentIndex = nit.Increment())
    {
			GraphNode* pCurrent = m_pGraph->GetNodeManager().GetNode(currentIndex);
			if (pCurrent->GetWaypointNavData()->nBuildingID == nBuildingID && !GetAISystem()->ExitNodeImpossible(m_pGraph->GetLinkManager(), pCurrent, 0.4f))
      {
        float dist = Distance::Point_Point(pos, pCurrent->GetPos());
        nodes.push_back(std::make_pair(dist, currentIndex)); // dist calculated later
      }
    }
  }

  if (nodes.empty())
  {
    AIWarning("CWaypointHumanNavRegion::GetTeleportPosition No nodes found for waypoint area containing %s (%5.2f, %5.2f, %5.2f) building id %d (%s) [design bug]", 
      requesterName, pos.x, pos.y, pos.z, nBuildingID, gAIEnv.pNavigation->GetSpecialAreaName(nBuildingID));
    return false;
  }

  std::sort(nodes.begin(), nodes.end());

  for (TNodes::const_iterator it2 = nodes.begin() ; it2 != nodes.end() ; ++it2)
  {
		unsigned nodeIndex = it2->second;
    const GraphNode *pNode = m_pGraph->GetNodeManager().GetNode(nodeIndex);
    Vec3 nodePos = pNode->GetPos();
    if (!CheckBodyPos(nodePos, AICE_ALL))
      continue;
    if (GetAISystem()->WouldHumanBeVisible(nodePos, false))
      continue;
    teleportPos = nodePos;
    return true;
  }
  return false;
}

/*
void CWaypointHumanNavRegion::CheckLinkLengths() const
{
	LinkIterator linkIt = CreateLinkIterator();
	for ( ; linkIt; ++linkIt)
	{
		unsigned int linkIndex = *linkIt;
		unsigned nodeIndex0 = m_pGraph->GetLinkManager().GetPrevNode(linkIndex);
		unsigned nodeIndex1 = m_pGraph->GetLinkManager().GetNextNode(linkIndex);
		GraphNode * node0 = m_pGraph->GetNodeManager().GetNode(nodeIndex0);
		GraphNode * node1 = m_pGraph->GetNodeManager().GetNode(nodeIndex1);
		AIWarning ("Link length %f\n", (node0->GetPos() - node1->GetPos()).len());
	}
}
*/

CWaypointHumanNavRegion::LinkIterator::LinkIterator (const CWaypointHumanNavRegion * wayptRegion) :
		m_pGraph (wayptRegion->m_pGraph),
		m_currNodeIt(new CAllNodesContainer::Iterator(m_pGraph->GetAllNodes(), IAISystem::NAV_WAYPOINT_HUMAN)),
		m_currLinkIndex (m_pGraph->GetNodeManager().GetNode(m_currNodeIt->GetNodeIndex())->firstLinkIndex)
{
	FindNextWaypointLink();
}

CWaypointHumanNavRegion::LinkIterator::LinkIterator (const LinkIterator & rhs) :
		m_pGraph (rhs.m_pGraph),
		m_currNodeIt (new CAllNodesContainer::Iterator(*rhs.m_currNodeIt)),
		m_currLinkIndex (rhs.m_currLinkIndex)
{
}

CWaypointHumanNavRegion::LinkIterator & CWaypointHumanNavRegion::LinkIterator::operator++ ()
{
	m_currLinkIndex = m_pGraph->GetLinkManager().GetNextLink(m_currLinkIndex);
	FindNextWaypointLink();
	return *this;
}

/**
 * This function finds the next waypoint link, starting with and including the
 * current value of m_currLinkIndex.  m_currLinkIndex should denote
 * a valid link upon the call even though not necessarily a waypoint one.
 * Note that if m_currLinkIndex does point to a waypoint link, this function
 * does nothing.
 */
void CWaypointHumanNavRegion::LinkIterator::FindNextWaypointLink()
{
	unsigned int currNodeIndex = m_currNodeIt->GetNodeIndex();
	bool gotSuitableLink = false;

	while (! gotSuitableLink)
	{
		while (m_currLinkIndex == 0)
		{
			m_currNodeIt->Increment();
			currNodeIndex = m_currNodeIt->GetNodeIndex();
			if (0 == currNodeIndex)
				return;		// reached the final node, the iteration is over

			m_currLinkIndex = m_pGraph->GetNodeManager().GetNode(currNodeIndex)->firstLinkIndex;
		}

		unsigned nextNodeIndex = m_pGraph->GetLinkManager().GetNextNode(m_currLinkIndex);

		const GraphNode * currNode = m_pGraph->GetNodeManager().GetNode(currNodeIndex);
		const GraphNode * nextNode = m_pGraph->GetNodeManager().GetNode(nextNodeIndex);

		if (nextNode->navType != IAISystem::NAV_WAYPOINT_HUMAN || currNode > nextNode /*check in one direction only*/)
		{
			m_currLinkIndex = m_pGraph->GetLinkManager().GetNextLink(m_currLinkIndex);
			continue;
		}

		gotSuitableLink = true;
	}
}

//====================================================================
// GetCachedPassabilityResult
//====================================================================
SCachedPassabilityResult* CWaypointHumanNavRegion::GetCachedPassabilityResult(size_t graphLink)
{
	PassabilityCache::iterator it = m_passabilityCache.find(graphLink);
	if (it != m_passabilityCache.end())
		return &(*it).second;

	return 0;
}


void CWaypointHumanNavRegion::InsertCachedPassabilityResult(size_t graphLink, const SCachedPassabilityResult& cachedResult)
{
	std::pair<PassabilityCache::iterator, bool> it = 
		m_passabilityCache.insert(PassabilityCache::value_type(graphLink, cachedResult));

	if (!it.second)
		it.first->second = cachedResult;
}